#include "Matrix.h"
#include <cassert>
#include <string>



Matrix::Matrix()
{
	memset(m_matrix, 0, sizeof(float) * 16);
}

void Matrix::Zero()
{
	memset(m_matrix, 0, sizeof(float) * 16);
}

Matrix::Matrix(const float* rhs) 
{
	memcpy(m_matrix, rhs, sizeof(float) * 16);
}

Matrix::Matrix(const Quaternion& rhs)
{
	m_matrix[ 0] = 1.0f - 2.0f * ( rhs.m_y * rhs.m_y + rhs.m_z * rhs.m_z ); 
	m_matrix[ 1] = 2.0f * (rhs.m_x * rhs.m_y + rhs.m_z * rhs.m_w);
	m_matrix[ 2] = 2.0f * (rhs.m_x * rhs.m_z - rhs.m_y * rhs.m_w);
	m_matrix[ 3] = 0.0f;  

	m_matrix[ 4] = 2.0f * ( rhs.m_x * rhs.m_y - rhs.m_z * rhs.m_w );  
	m_matrix[ 5] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_z * rhs.m_z ); 
	m_matrix[ 6] = 2.0f * (rhs.m_z * rhs.m_y + rhs.m_x * rhs.m_w );  
	m_matrix[ 7] = 0.0f;  

	m_matrix[ 8] = 2.0f * ( rhs.m_x * rhs.m_z + rhs.m_y * rhs.m_w );
	m_matrix[ 9] = 2.0f * ( rhs.m_y * rhs.m_z - rhs.m_x * rhs.m_w );
	m_matrix[10] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_y * rhs.m_y );  
	m_matrix[11] = 0.0f;  

	m_matrix[12] = 0;  
	m_matrix[13] = 0;  
	m_matrix[14] = 0;  
	m_matrix[15] = 1.0f;
}

Matrix& Matrix::operator=(const Quaternion& rhs)
{
	m_matrix[ 0] = 1.0f - 2.0f * ( rhs.m_y * rhs.m_y + rhs.m_z * rhs.m_z ); 
	m_matrix[ 1] = 2.0f * (rhs.m_x * rhs.m_y + rhs.m_z * rhs.m_w);
	m_matrix[ 2] = 2.0f * (rhs.m_x * rhs.m_z - rhs.m_y * rhs.m_w);
	m_matrix[ 3] = 0.0f;  

	m_matrix[ 4] = 2.0f * ( rhs.m_x * rhs.m_y - rhs.m_z * rhs.m_w );  
	m_matrix[ 5] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_z * rhs.m_z ); 
	m_matrix[ 6] = 2.0f * (rhs.m_z * rhs.m_y + rhs.m_x * rhs.m_w );  
	m_matrix[ 7] = 0.0f;  

	m_matrix[ 8] = 2.0f * ( rhs.m_x * rhs.m_z + rhs.m_y * rhs.m_w );
	m_matrix[ 9] = 2.0f * ( rhs.m_y * rhs.m_z - rhs.m_x * rhs.m_w );
	m_matrix[10] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_y * rhs.m_y );  
	m_matrix[11] = 0.0f;  

	m_matrix[12] = 0;  
	m_matrix[13] = 0;  
	m_matrix[14] = 0;  
	m_matrix[15] = 1.0f;

	return (*this);
}

void Matrix::Identity()
{
	for(unsigned int n = 0; n < 16; ++n)
	{
		m_matrix[n] = 0.0f;
	}

	m_matrix[0] = m_matrix[5] = m_matrix[10] = m_matrix[15] = 1.0f;
}

Matrix& Matrix::operator*=(const Matrix& rhs)
{
	float temp[16];

	temp[0] = (m_matrix[0]  * rhs.m_matrix[0])  +
		(m_matrix[4]  * rhs.m_matrix[1])  +
		(m_matrix[8]  * rhs.m_matrix[2])  +
		(m_matrix[12] * rhs.m_matrix[3]);

	temp[4] = (m_matrix[0]  * rhs.m_matrix[4])  +
		(m_matrix[4]  * rhs.m_matrix[5])  +
		(m_matrix[8]  * rhs.m_matrix[6])  +
		(m_matrix[12] * rhs.m_matrix[7]);

	temp[8] = (m_matrix[0]  * rhs.m_matrix[8])  +
		(m_matrix[4]  * rhs.m_matrix[9])  +
		(m_matrix[8]  * rhs.m_matrix[10]) +
		(m_matrix[12] * rhs.m_matrix[11]);

	temp[12] =(m_matrix[0]  * rhs.m_matrix[12]) +
		(m_matrix[4]  * rhs.m_matrix[13]) +
		(m_matrix[8]  * rhs.m_matrix[14]) +
		(m_matrix[12] * rhs.m_matrix[15]); 

	temp[1] = (m_matrix[1]  * rhs.m_matrix[0])  +
		(m_matrix[5]  * rhs.m_matrix[1])  +
		(m_matrix[9]  * rhs.m_matrix[2])  +
		(m_matrix[13] * rhs.m_matrix[3]);

	temp[5] = (m_matrix[1]  * rhs.m_matrix[4])  +
		(m_matrix[5]  * rhs.m_matrix[5])  +
		(m_matrix[9]  * rhs.m_matrix[6])  +
		(m_matrix[13] * rhs.m_matrix[7]);

	temp[9] = (m_matrix[1]  * rhs.m_matrix[8])  +
		(m_matrix[5]  * rhs.m_matrix[9])  +
		(m_matrix[9]  * rhs.m_matrix[10]) +
		(m_matrix[13] * rhs.m_matrix[11]);

	temp[13] =(m_matrix[1]  * rhs.m_matrix[12]) +
		(m_matrix[5]  * rhs.m_matrix[13]) +
		(m_matrix[9]  * rhs.m_matrix[14]) +
		(m_matrix[13] * rhs.m_matrix[15]); 

	temp[2] = (m_matrix[2]  * rhs.m_matrix[0])  +
		(m_matrix[6]  * rhs.m_matrix[1])  +
		(m_matrix[10] * rhs.m_matrix[2])  +
		(m_matrix[14] * rhs.m_matrix[3]);

	temp[6] = (m_matrix[2]  * rhs.m_matrix[4])  +
		(m_matrix[6]  * rhs.m_matrix[5])  +
		(m_matrix[10] * rhs.m_matrix[6])  +
		(m_matrix[14] * rhs.m_matrix[7]);

	temp[10]= (m_matrix[2]  * rhs.m_matrix[8])  +
		(m_matrix[6]  * rhs.m_matrix[9])  +
		(m_matrix[10] * rhs.m_matrix[10]) +
		(m_matrix[14] * rhs.m_matrix[11]);

	temp[14]= (m_matrix[2]  * rhs.m_matrix[12]) +
		(m_matrix[6]  * rhs.m_matrix[13]) +
		(m_matrix[10] * rhs.m_matrix[14]) +
		(m_matrix[14] * rhs.m_matrix[15]); 

	temp[3]= (m_matrix[3]  * rhs.m_matrix[0])  +
		(m_matrix[7]  * rhs.m_matrix[1])  +
		(m_matrix[11] * rhs.m_matrix[2])  +
		(m_matrix[15] * rhs.m_matrix[3]);

	temp[7]= (m_matrix[3]  * rhs.m_matrix[4])  +
		(m_matrix[7]  * rhs.m_matrix[5])  +
		(m_matrix[11] * rhs.m_matrix[6])  +
		(m_matrix[15] * rhs.m_matrix[7]);

	temp[11]= (m_matrix[3]  * rhs.m_matrix[8])  +
		(m_matrix[7]  * rhs.m_matrix[9])  +
		(m_matrix[11] * rhs.m_matrix[10]) +
		(m_matrix[15] * rhs.m_matrix[11]);

	temp[15]= (m_matrix[3]  * rhs.m_matrix[12]) +
		(m_matrix[7]  * rhs.m_matrix[13]) +
		(m_matrix[11] * rhs.m_matrix[14]) +
		(m_matrix[15] * rhs.m_matrix[15]); 

	for(unsigned int n = 0; n < 16; ++n)
	{
		m_matrix[n] = temp[n];
	}

	return (*this);
}

Matrix& Matrix::operator*=(const float* f)
{
	float temp[16];

	temp[0] = (m_matrix[0]  * f[0])  +
		(m_matrix[4]  * f[1])  +
		(m_matrix[8]  * f[2])  +
		(m_matrix[12] * f[3]);

	temp[1] = (m_matrix[0]  * f[4])  +
		(m_matrix[4]  * f[5])  +
		(m_matrix[8]  * f[6])  +
		(m_matrix[12] * f[7]);

	temp[2] = (m_matrix[0]  * f[8])  +
		(m_matrix[4]  * f[9])  +
		(m_matrix[8]  * f[10]) +
		(m_matrix[12] * f[11]);

	temp[3] = (m_matrix[0]  * f[12]) +
		(m_matrix[4]  * f[13]) +
		(m_matrix[8]  * f[14]) +
		(m_matrix[12] * f[15]); 

	temp[4] = (m_matrix[1]  * f[0])  +
		(m_matrix[5]  * f[1])  +
		(m_matrix[9]  * f[2])  +
		(m_matrix[13] * f[3]);

	temp[5] = (m_matrix[1]  * f[4])  +
		(m_matrix[5]  * f[5])  +
		(m_matrix[9]  * f[6])  +
		(m_matrix[13] * f[7]);

	temp[6] = (m_matrix[1]  * f[8])  +
		(m_matrix[5]  * f[9])  +
		(m_matrix[9]  * f[10]) +
		(m_matrix[13] * f[11]);

	temp[7] = (m_matrix[1]  * f[12]) +
		(m_matrix[5]  * f[13]) +
		(m_matrix[9]  * f[14]) +
		(m_matrix[13] * f[15]); 

	temp[8] = (m_matrix[2]  * f[0])  +
		(m_matrix[6]  * f[1])  +
		(m_matrix[10] * f[2])  +
		(m_matrix[14] * f[3]);

	temp[9] = (m_matrix[2]  * f[4])  +
		(m_matrix[6]  * f[5])  +
		(m_matrix[10] * f[6])  +
		(m_matrix[14] * f[7]);

	temp[10]= (m_matrix[2]  * f[8])  +
		(m_matrix[6]  * f[9])  +
		(m_matrix[10] * f[10]) +
		(m_matrix[14] * f[11]);

	temp[11]= (m_matrix[2]  * f[12]) +
		(m_matrix[6]  * f[13]) +
		(m_matrix[10] * f[14]) +
		(m_matrix[14] * f[15]); 

	temp[12]= (m_matrix[3]  * f[0])  +
		(m_matrix[7]  * f[1])  +
		(m_matrix[11] * f[2])  +
		(m_matrix[15] * f[3]);

	temp[13]= (m_matrix[3]  * f[4])  +
		(m_matrix[7]  * f[5])  +
		(m_matrix[11] * f[6])  +
		(m_matrix[15] * f[7]);

	temp[14]= (m_matrix[3]  * f[8])  +
		(m_matrix[7]  * f[9])  +
		(m_matrix[11] * f[10]) +
		(m_matrix[15] * f[11]);

	temp[15]= (m_matrix[3]  * f[12]) +
		(m_matrix[7]  * f[13]) +
		(m_matrix[11] * f[14]) +
		(m_matrix[15] * f[15]); 

	for(unsigned int n = 0; n < 16; ++n)
	{
		m_matrix[n] = temp[n];
	}

	return (*this);
}

Matrix Matrix::operator*(const Matrix& rhs)
{
	Matrix temp = rhs;
	temp *= (*this);

	return temp;
}

Matrix Matrix::operator*(const float* f)
{
	Matrix temp = (*this);
	temp *= f;

	return temp;
}

Vec3 Matrix::operator*(const Vec3& f)
{
	float v4[4];

	v4[0] = (m_matrix[0]  * f.m_x)  +
		(m_matrix[4]  * f.m_y)  +
		(m_matrix[8]  * f.m_z)  +
		(m_matrix[12]);

	v4[1]  = (m_matrix[1]  * f.m_x)  +
		(m_matrix[5]  * f.m_y)  +
		(m_matrix[9]  * f.m_z)  +
		(m_matrix[13]);

	v4[2]  = (m_matrix[2]  * f.m_x)  +
		(m_matrix[6]  * f.m_y)  +
		(m_matrix[10] * f.m_z)  +
		(m_matrix[14]);

	v4[3]  = (m_matrix[3]  * f.m_x)  +
		(m_matrix[7]  * f.m_y)  +
		(m_matrix[11] * f.m_z)  +
		(m_matrix[15]);

	return Vec3(v4[0] / v4[3], v4[1] / v4[3], v4[2] / v4[3]);
}

void Matrix::SetTranslation(const Vec3& t)
{
	m_matrix[12] = t.m_x;
	m_matrix[13] = t.m_y;
	m_matrix[14] = t.m_z;
	m_matrix[15] = 1.0f;
}

void Matrix::SetRotationX(float rads)
{
	m_matrix[0] = 1.0f;
	m_matrix[5] = std::cos(rads);
	m_matrix[6] = std::sin(rads);
	m_matrix[9] = -std::sin(rads);
	m_matrix[10] = std::cos(rads);
	m_matrix[15] = 1.0f;
}

void Matrix::SetRotationY(float rads)
{
	m_matrix[0] = std::cos(rads);
	m_matrix[2] = -std::sin(rads);
	m_matrix[5] = 1.0f;
	m_matrix[8] = std::sin(rads);
	m_matrix[10] = std::cos(rads);
	m_matrix[15] = 1.0f;
}

void Matrix::SetRotationZ(float rads)
{
	m_matrix[0] = std::cos(rads);
	m_matrix[1] = std::sin(rads);
	m_matrix[4] = -std::sin(rads);
	m_matrix[5] = std::cos(rads);
	m_matrix[10] = 1.0f;
	m_matrix[15] = 1.0f;
}

void Matrix::SetScale(const Vec3& s)
{
	m_matrix[0] = s.m_x;
	m_matrix[5] = s.m_y;
	m_matrix[10] = s.m_z;
	m_matrix[15] = 1.0f;
}

float& Matrix::operator[](unsigned int n)
{
	assert(n < 16);
	return m_matrix[n];
}

void Matrix::Transpose()
{
	std::swap(m_matrix[1], m_matrix[4]);
	std::swap(m_matrix[2], m_matrix[8]);
	std::swap(m_matrix[6], m_matrix[9]);
	std::swap(m_matrix[3], m_matrix[12]);
	std::swap(m_matrix[7], m_matrix[13]);
	std::swap(m_matrix[11], m_matrix[14]);
}

/*
Matrix Matrix::Inverse() const
{
float inv[16];
float det[16];

int i;

inv[0] = m_matrix[5]  * m_matrix[10] * m_matrix[15] - 
m_matrix[5]  * m_matrix[11] * m_matrix[14] - 
m_matrix[9]  * m_matrix[6]  * m_matrix[15] + 
m_matrix[9]  * m_matrix[7]  * m_matrix[14] +
m_matrix[13] * m_matrix[6]  * m_matrix[11] - 
m_matrix[13] * m_matrix[7]  * m_matrix[10];

inv[4] = -m_matrix[4]  * m_matrix[10] * m_matrix[15] + 
m_matrix[4]  * m_matrix[11] * m_matrix[14] + 
m_matrix[8]  * m_matrix[6]  * m_matrix[15] - 
m_matrix[8]  * m_matrix[7]  * m_matrix[14] - 
m_matrix[12] * m_matrix[6]  * m_matrix[11] + 
m_matrix[12] * m_matrix[7]  * m_matrix[10];

inv[8] = m_matrix[4]  * m_matrix[9]  * m_matrix[15] - 
m_matrix[4]  * m_matrix[11] * m_matrix[13] - 
m_matrix[8]  * m_matrix[5]  * m_matrix[15] + 
m_matrix[8]  * m_matrix[7]  * m_matrix[13] + 
m_matrix[12] * m_matrix[5]  * m_matrix[11] - 
m_matrix[12] * m_matrix[7]  * m_matrix[9];

inv[12] = -m[4]  * m[9] * m[14] + 
m[4]  * m[10] * m[13] +
m[8]  * m[5] * m[14] - 
m[8]  * m[6] * m[13] - 
m[12] * m[5] * m[10] + 
m[12] * m[6] * m[9];

inv[1] = -m[1]  * m[10] * m[15] + 
m[1]  * m[11] * m[14] + 
m[9]  * m[2] * m[15] - 
m[9]  * m[3] * m[14] - 
m[13] * m[2] * m[11] + 
m[13] * m[3] * m[10];

inv[5] = m[0]  * m[10] * m[15] - 
m[0]  * m[11] * m[14] - 
m[8]  * m[2] * m[15] + 
m[8]  * m[3] * m[14] + 
m[12] * m[2] * m[11] - 
m[12] * m[3] * m[10];

inv[9] = -m[0]  * m[9] * m[15] + 
m[0]  * m[11] * m[13] + 
m[8]  * m[1] * m[15] - 
m[8]  * m[3] * m[13] - 
m[12] * m[1] * m[11] + 
m[12] * m[3] * m[9];

inv[13] = m[0]  * m[9] * m[14] - 
m[0]  * m[10] * m[13] - 
m[8]  * m[1] * m[14] + 
m[8]  * m[2] * m[13] + 
m[12] * m[1] * m[10] - 
m[12] * m[2] * m[9];

inv[2] = m[1]  * m[6] * m[15] - 
m[1]  * m[7] * m[14] - 
m[5]  * m[2] * m[15] + 
m[5]  * m[3] * m[14] + 
m[13] * m[2] * m[7] - 
m[13] * m[3] * m[6];

inv[6] = -m[0]  * m[6] * m[15] + 
m[0]  * m[7] * m[14] + 
m[4]  * m[2] * m[15] - 
m[4]  * m[3] * m[14] - 
m[12] * m[2] * m[7] + 
m[12] * m[3] * m[6];

inv[10] = m[0]  * m[5] * m[15] - 
m[0]  * m[7] * m[13] - 
m[4]  * m[1] * m[15] + 
m[4]  * m[3] * m[13] + 
m[12] * m[1] * m[7] - 
m[12] * m[3] * m[5];

inv[14] = -m[0]  * m[5] * m[14] + 
m[0]  * m[6] * m[13] + 
m[4]  * m[1] * m[14] - 
m[4]  * m[2] * m[13] - 
m[12] * m[1] * m[6] + 
m[12] * m[2] * m[5];

inv[3] = -m[1] * m[6] * m[11] + 
m[1] * m[7] * m[10] + 
m[5] * m[2] * m[11] - 
m[5] * m[3] * m[10] - 
m[9] * m[2] * m[7] + 
m[9] * m[3] * m[6];

inv[7] = m[0] * m[6] * m[11] - 
m[0] * m[7] * m[10] - 
m[4] * m[2] * m[11] + 
m[4] * m[3] * m[10] + 
m[8] * m[2] * m[7] - 
m[8] * m[3] * m[6];

inv[11] = -m[0] * m[5] * m[11] + 
m[0] * m[7] * m[9] + 
m[4] * m[1] * m[11] - 
m[4] * m[3] * m[9] - 
m[8] * m[1] * m[7] + 
m[8] * m[3] * m[5];

inv[15] = m[0] * m[5] * m[10] - 
m[0] * m[6] * m[9] - 
m[4] * m[1] * m[10] + 
m[4] * m[2] * m[9] + 
m[8] * m[1] * m[6] - 
m[8] * m[2] * m[5];

det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

if (det == 0)
return false;

det = 1.0 / det;

for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;

return true;
}

return temp;
}
*/

